#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"

float PI=3.14159265;
float dg,dob,sig,sio,v_o,w_o;
class RunTest:public rclcpp::Node
{
    private:
        // 声名定时器指针
        rclcpp::TimerBase::SharedPtr timer_;
        // 声明话题发布者指针
        rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publisher_;
        void timer_callback()
        {
            // 创建消息
            std_msgs::msg::Float32MultiArray message;
            message.data.resize(6);
            dg=8.0;
            message.data[0]=dg;
            dob=3.0;
            message.data[1]=dob;
            sig=PI/6.0;
            message.data[2]=sig;
            sio=PI/2.0;
            message.data[3]=sio;
            v_o=0.4;
            message.data[4]=v_o;
            w_o=PI/180.0;
            message.data[5]=w_o;
            // 日志打印
            RCLCPP_INFO(this->get_logger(), "dg=%f, do=%f, sig=%f, sio=%f", dg,dob,sig,sio);

            // 发布消息
            command_publisher_->publish(message);
        }
    public:
        RunTest(std::string name):Node(name)
        {
            RCLCPP_INFO(this->get_logger(), "大家好，我是%s", name.c_str());
            // 创建发布者
            command_publisher_=this->create_publisher<std_msgs::msg::Float32MultiArray>("poRec",10);
            // 创建定时器，500ms为周期，定时发布
            RCLCPP_INFO(this->get_logger(), "定时器ON");
            timer_=this->create_wall_timer(std::chrono::milliseconds(500),std::bind(&RunTest::timer_callback,this));
        }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<RunTest>("run_test");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}